#include "mixer.h"
#include "defines.h"

// The array where the autopilot commands are put before mixing.
float APOutputCommands[APCON_MAX_COMMANDS];

// A count of the number of mixers used.
// IT IS VERY IMPORTANT TO SET THIS CORRECTLY. FAILURE MEANS UNKNOWN BEHAVOIR.
const unsigned int MixerCount = 12;

// Mixer settings
const MixerSettings Mixers[MAX_MIXERS] = {
		1,1,	RUDDER_INPUT_CHANNEL,	RUDDER_OUTPUT_CHANNEL	,1,1,0,
		1,1,	ELEVATOR_INPUT_CHANNEL,	ELEVATOR_OUTPUT_CHANNEL	,1,1,0,
		1,1,	THROTTLE_INPUT_CHANNEL,	THROTTLE_OUTPUT_CHANNEL	,1,1,0,
		1,1,	AILERON_INPUT_CHANNEL,	AILERON_OUTPUT_CHANNEL	,1,1,0,
		1,1,	AILERON2_INPUT_CHANNEL,	AILERON_SECONDARY_OUTPUT_CHANNEL ,1,1,0,
		1,1,	FLAP1_INPUT_CHANNEL,	FLAP1_OUTPUT_CHANNEL	,1,1,0,
		1,1,	FLAP2_INPUT_CHANNEL,	FLAP2_OUTPUT_CHANNEL	,1,1,0,

		1,1,	APCON_THROTTLE,			THROTTLE_OUTPUT_CHANNEL		,0,0,0,
		-1.0,-1.0,APCON_YAW,			RUDDER_OUTPUT_CHANNEL		,0,0,0,
		0.5,0.5,APCON_PITCH,			ELEVATOR_OUTPUT_CHANNEL		,0,0,0,
		0.5,1.0,APCON_ROLL,				AILERON_OUTPUT_CHANNEL		,0,0,0,
		1.0,0.5,APCON_ROLL,				AILERON_SECONDARY_OUTPUT_CHANNEL ,0,0,0,	};

const WAGGLE_SETTINGS waggleSettings = {1,0,0};


void servoMix( void )
{
	int MixerNo;		// Index of mixer in mixer list
	int Destination;	// Index of destination channel
	int Source;			// Index of source channel for hte mixer

	MixerSettings* pMixSettings = &Mixers;

	float SourceVal;
	float DestVal;
	float MixVal;
	float Coefficient;

	int DestTemp[MAX_OUTPUTS+1];	// Will put sum of mixer in here.

// Setup all destinations to trim points for safety
	for(Destination = 0; Destination < MAX_OUTPUTS; Destination++)
	{
		DestTemp[Destination] = udb_pwTrim[Destination];
	};

// Step through each mixer.  Find source and destination for each mixer
	for(MixerNo = 0; MixerNo < MixerCount; MixerNo++)
	{
		pMixSettings = &(Mixers[MixerNo]);

		Source 		= pMixSettings->Source;
		Destination = pMixSettings->Destination;

		// Source value is always offset from zero.
		if(pMixSettings->SourceIsInput == 1)
		{
			if (udb_flags._.radio_on)
			{	
				SourceVal = udb_pwIn[Source] - udb_pwTrim[Source];	// Remove zero trim point for inputs.
			}
			else
			{
				SourceVal = 0;										// No input if radio is off, trim addded later
			}
		}
		else
		{
			SourceVal = (float) autopilot_controls.controlArray[Source];

			// This controls the waggles. 
			// Waggle the autopilot controls because we know how they mix to the outputs
			// Add waggle if it is not zero to save run time.
			if(waggle != 0)
			{
				if( ((Source == APCON_YAW) && (waggleSettings.waggleYawControl == 1)) ||
					((Source == APCON_ROLL) && (waggleSettings.waggleRollControl == 1)) ||
					((Source == APCON_PITCH) && (waggleSettings.wagglePitchControl == 1)) )
				{
					SourceVal += waggle;
				};
			};	
		};

		// Pick the positive or negative coefficient.
		if(SourceVal >= 0)
			Coefficient = pMixSettings->PosCoeff;
		else
			Coefficient = pMixSettings->NegCoeff;

		// Mix
		MixVal = Coefficient * SourceVal;

		// If forcing the value instead of mixing, add the trim offset back to the mix.
		if(pMixSettings->ForceValue == 1)
			DestTemp[Destination] = MixVal + udb_pwTrim[Source];
		else
			DestTemp[Destination] += MixVal;
	};

// Loop through temporary outputs and adjust for servo saturation.
	for(Destination = 0; Destination < MAX_OUTPUTS; Destination++)
	{
		DestTemp[Destination] = udb_servo_pulsesat(DestTemp[Destination]);
	};

	memcpy(&udb_pwOut, &DestTemp, sizeof(udb_pwOut));
};


